#include "../System/config.h"

#include "./BSP/DEBUG/debug.h"
#include "./BSP/KEY/key.h"
#include "./BSP/LED/led.h"
#include "./BSP/TRACK/track.h"
#include "./BSP/JYxx/jyxx.h"
#include "./BSP/MOTOR/motor.h"
#include "./BSP/ENCODER/encoder.h"
#include "./BSP/LED/led.h"

#include "FreeRTOS.h"
#include "task.h"

#include "./MALLOC/malloc.h"
#include "./PID/pid.h"

#include "app_motor.h"
#include "app_key.h"

PID_T pid_speed_left;  // 左轮速度环
PID_T pid_speed_right; // 右轮速度环

PID_T pid_line;  // 循迹环
PID_T pid_angle; // 角度环

int basic_speed = 20; // 基础速度

// 低通滤波器系数 (Low-pass filter coefficient 'alpha')
// alpha 越小, 滤波效果越强, 但延迟越大。建议从 0.1 到 0.5 之间开始尝试。
#define SPEED_FILTER_ALPHA_LEFT 0.15f
#define SPEED_FILTER_ALPHA_RIGHT 0.15f

// 用于存储滤波后速度的变量
static float filtered_speed_left = 0.0f;
static float filtered_speed_right = 0.0f;

PidParams_t pid_params_speed_left = {
    .kp = 1.0f,  // 减小比例系数
    .ki = 0.08f, // 增大积分系数
    .kd = 1.5f,  // 减小微分系数
    .out_min = -100,
    .out_max = 100,
};

PidParams_t pid_params_speed_right = {
    .kp = 1.0f,  // 增量式PID的P参数
    .ki = 0.08f, // 增量式PID的I参数  不适宜给太大 因为他会积累误差
    .kd = 1.5f,  // 增量式PID的D参数，不宜过大
    .out_min = -100,
    .out_max = 100,
};

// 巡线控制PID (位置式)
PidParams_t pid_params_line = {
    .kp = 3.0f,
    .ki = 0.0001f,
    .kd = 1.0f,
    .out_min = -100,
    .out_max = 100,
};

// 角度环（偏航角）
PidParams_t pid_params_angle = {
    .kp = 1.0f,
    .ki = 0.00001f,
    .kd = 0.100f,
    .out_min = -100,
    .out_max = 100,
};

void pid_test(uint16_t p, uint16_t i, uint16_t d)
{
    float pp = (float)p / 100.0f;
    float ii = (float)i / 100.0f;
    float dd = (float)d / 100.0f;
    pid_params_angle.kp = pp;
    pid_params_angle.ki = ii;
    pid_params_angle.kd = dd;

    pid_init(&pid_line,
             pid_params_angle.kp, pid_params_angle.ki, pid_params_angle.kd,
             0.0f, pid_params_angle.out_max);
}

void motor_pid_init(void)
{
    // 初始化左轮速度PID控制器
    pid_init(&pid_speed_left,
             pid_params_speed_left.kp, pid_params_speed_left.ki, pid_params_speed_left.kd,
             0.0f, pid_params_speed_left.out_max);

    // 初始化右轮速度PID控制器
    pid_init(&pid_speed_right,
             pid_params_speed_right.kp, pid_params_speed_right.ki, pid_params_speed_right.kd,
             0.0f, pid_params_speed_right.out_max);

    // 初始化直线行驶PID控制器
    pid_init(&pid_line,
             pid_params_line.kp, pid_params_line.ki, pid_params_line.kd,
             0.0f, pid_params_line.out_max);

    pid_init(&pid_angle,
             pid_params_angle.kp, pid_params_angle.ki, pid_params_angle.kd,
             0.0f, pid_params_angle.out_max);

    // 设置左轮速度PID控制器的目标值
    pid_set_target(&pid_speed_left, basic_speed);
    // 设置右轮速度PID控制器的目标值
    pid_set_target(&pid_speed_right, basic_speed);

    pid_set_target(&pid_line, 0);

    pid_set_target(&pid_angle, 0);
}

static uint8_t is_app_motor_task_create = 0;
static uint8_t motor_mode = 0;
uint8_t motor_led_state = 0;

/**
 * @brief       电机按键处理函数
 * @param       key_event: 按键事件
 * @param       key_value: 按键值
 * @retval      无
 */
static void app_motor_key_handle(uint8_t key_event, uint8_t key_value)
{
    if (key_event == KEY_EVENT_LONG_PRESS)
    {
        if (key_value == 1)
        {
            motor_mode = 0;
        }
        else if (key_value == 2)
        {
        }
        else if (key_value == 3)
        {
        }
        else if (key_value == 4)
        {
        }
    }

    if (key_event == KEY_EVENT_CLICK)
    {
        if (key_value == 1)
        {
            motor_mode = 1;
        }
        else if (key_value == 2)
        {
            motor_mode = 2;
        }
        else if (key_value == 3)
        {
            motor_mode = 3;
        }
        else if (key_value == 4)
        {
            motor_mode = 4;
        }
        else if (key_value == 5)
        {
            motor_mode = 0;
        }
    }
}

void Line_PID_control(uint8_t data) // 循迹环控制
{
    int line_pid_output = 0;
    line_pid_output = pid_calculate_incremental(&pid_line, track_get_line_position_error(data));
    line_pid_output = pid_constrain(line_pid_output, pid_params_line.out_min, pid_params_line.out_max);

    pid_set_target(&pid_speed_left, basic_speed + line_pid_output);
    pid_set_target(&pid_speed_right, basic_speed - line_pid_output);
}

void Angle_PID_control(float yaw) // 角度环控制
{
    int angle_pid_output = 0;
    angle_pid_output = pid_calculate_incremental(&pid_angle, yaw);
    angle_pid_output = pid_constrain(angle_pid_output, pid_params_angle.out_min, pid_params_angle.out_max);
    pid_set_target(&pid_speed_left, basic_speed + 0.8 * angle_pid_output);
    pid_set_target(&pid_speed_right, basic_speed - 0.8 * angle_pid_output);
}

void motor_mode1(void)
{

    uint8_t track_data = track_get_sensor_data();
    if (track_data != 0) // 进入黑线
    {
        motor_led_state = 1;
        motor_mode = 0;
        return;
    }

    float *angle = jyxx_get_angle();
    Angle_PID_control(angle[2]);

    float left_encoder_speed = motor_get_speed(CFG_MOTOR_L_ID, 5);
    float right_encoder_speed = motor_get_speed(CFG_MOTOR_R_ID, 5);

    filtered_speed_left = SPEED_FILTER_ALPHA_LEFT * left_encoder_speed + (1.0f - SPEED_FILTER_ALPHA_LEFT) * filtered_speed_left;
    filtered_speed_right = SPEED_FILTER_ALPHA_RIGHT * right_encoder_speed + (1.0f - SPEED_FILTER_ALPHA_RIGHT) * filtered_speed_right;

    float output_speed_left = pid_calculate_incremental(&pid_speed_left, filtered_speed_left);
    float output_speed_right = pid_calculate_incremental(&pid_speed_right, filtered_speed_right);

    output_speed_left = pid_constrain(output_speed_left, pid_params_speed_left.out_min, pid_params_speed_left.out_max);
    output_speed_right = pid_constrain(output_speed_right, pid_params_speed_right.out_min, pid_params_speed_right.out_max);

    my_printf("left_encoder_speed=%.2f,output_speed_left=%.2f\r\n", left_encoder_speed, output_speed_left);
    my_printf("right_encoder_speed=%.2f,output_speed_right=%.2f\r\n", right_encoder_speed, output_speed_right);

    motor_set_speed(CFG_MOTOR_R_ID, output_speed_right);
    motor_set_speed(CFG_MOTOR_L_ID, output_speed_left);
}

void motor_mode2(void)
{
    static uint8_t car_state = 'A';
    uint8_t track_data = track_get_sensor_data();

    if (car_state == 'A')
    {
        pid_set_target(&pid_angle, 0);
        float *angle = jyxx_get_angle();
        Angle_PID_control(angle[2]);

        if (track_data != 0) // 进入黑线
        {
            car_state = 'B';
            motor_led_state = 1;
        }
    }

    else if (car_state == 'B')
    {
        Line_PID_control(track_data);
        if (track_data == 0)
        {
            car_state = 'C';
            motor_led_state = 1;
        }
    }
    else if (car_state == 'C')
    {
        pid_set_target(&pid_angle, -180);
        float *angle = jyxx_get_angle();
        Angle_PID_control(angle[2]);

        if (track_data != 0) // 进入黑线
        {
            car_state = 'D';
            motor_led_state = 1;
        }
    }
    else if (car_state == 'D')
    {
        Line_PID_control(track_data);
        if (track_data == 0)
        {
            car_state = 'A';
            motor_led_state = 1;
            motor_mode = 0;
            return;
        }
    }

    float left_encoder_speed = motor_get_speed(CFG_MOTOR_L_ID, 5);
    float right_encoder_speed = motor_get_speed(CFG_MOTOR_R_ID, 5);

    filtered_speed_left = SPEED_FILTER_ALPHA_LEFT * left_encoder_speed + (1.0f - SPEED_FILTER_ALPHA_LEFT) * filtered_speed_left;
    filtered_speed_right = SPEED_FILTER_ALPHA_RIGHT * right_encoder_speed + (1.0f - SPEED_FILTER_ALPHA_RIGHT) * filtered_speed_right;

    float output_speed_left = pid_calculate_incremental(&pid_speed_left, filtered_speed_left);
    float output_speed_right = pid_calculate_incremental(&pid_speed_right, filtered_speed_right);

    output_speed_left = pid_constrain(output_speed_left, pid_params_speed_left.out_min, pid_params_speed_left.out_max);
    output_speed_right = pid_constrain(output_speed_right, pid_params_speed_right.out_min, pid_params_speed_right.out_max);

    my_printf("left_encoder_speed=%.2f,output_speed_left=%.2f\r\n", left_encoder_speed, output_speed_left);
    my_printf("right_encoder_speed=%.2f,output_speed_right=%.2f\r\n", right_encoder_speed, output_speed_right);

    motor_set_speed(CFG_MOTOR_R_ID, output_speed_right);
    motor_set_speed(CFG_MOTOR_L_ID, output_speed_left);
}

void motor_mode3(void)
{
    static uint8_t car_state = 'A';
    uint8_t track_data = track_get_sensor_data();

    if (car_state == 'A')
    {
        pid_set_target(&pid_angle, -38.66);
        float *angle = jyxx_get_angle();
        Angle_PID_control(angle[2]);

        if (track_data != 0) // 进入黑线
        {
            car_state = 'C';
            basic_speed = 30;
            motor_led_state = 1;
        }
    }

    else if (car_state == 'C')
    {
        Line_PID_control(track_data);
        if (track_data == 0)
        {
            car_state = 'B';
            basic_speed = 20;
            motor_led_state = 1;
        }
    }
    else if (car_state == 'B')
    {
        pid_set_target(&pid_angle, 180 + 38.66);
        float *angle = jyxx_get_angle();
        Angle_PID_control(angle[2]);

        if (track_data != 0) // 进入黑线
        {
            basic_speed = 30;
            car_state = 'D';
            motor_led_state = 1;
        }
    }
    else if (car_state == 'D')
    {
        Line_PID_control(track_data);
        if (track_data == 0)
        {
            car_state = 'A';
            basic_speed = 20;
            motor_led_state = 1;
            motor_mode = 0;
            return;
        }
    }

    float left_encoder_speed = motor_get_speed(CFG_MOTOR_L_ID, 5);
    float right_encoder_speed = motor_get_speed(CFG_MOTOR_R_ID, 5);

    filtered_speed_left = SPEED_FILTER_ALPHA_LEFT * left_encoder_speed + (1.0f - SPEED_FILTER_ALPHA_LEFT) * filtered_speed_left;
    filtered_speed_right = SPEED_FILTER_ALPHA_RIGHT * right_encoder_speed + (1.0f - SPEED_FILTER_ALPHA_RIGHT) * filtered_speed_right;

    float output_speed_left = pid_calculate_incremental(&pid_speed_left, filtered_speed_left);
    float output_speed_right = pid_calculate_incremental(&pid_speed_right, filtered_speed_right);

    output_speed_left = pid_constrain(output_speed_left, pid_params_speed_left.out_min, pid_params_speed_left.out_max);
    output_speed_right = pid_constrain(output_speed_right, pid_params_speed_right.out_min, pid_params_speed_right.out_max);

    my_printf("left_encoder_speed=%.2f,output_speed_left=%.2f\r\n", left_encoder_speed, output_speed_left);
    my_printf("right_encoder_speed=%.2f,output_speed_right=%.2f\r\n", right_encoder_speed, output_speed_right);

    motor_set_speed(CFG_MOTOR_R_ID, output_speed_right);
    motor_set_speed(CFG_MOTOR_L_ID, output_speed_left);
}

void motor_mode4(void)
{
    static uint8_t car_state = 'A';
    uint8_t track_data = track_get_sensor_data();
    static uint8_t cnt = 0;
    if (car_state == 'A')
    {
        pid_set_target(&pid_angle, -38.66);
        float *angle = jyxx_get_angle();
        Angle_PID_control(angle[2]);

        if (track_data != 0) // 进入黑线
        {
            car_state = 'C';
            basic_speed = 30;
            motor_led_state = 1;
        }
    }

    else if (car_state == 'C')
    {
        Line_PID_control(track_data);
        if (track_data == 0)
        {
            car_state = 'B';
            basic_speed = 20;
            motor_led_state = 1;
        }
    }
    else if (car_state == 'B')
    {
        pid_set_target(&pid_angle, 180 + 38.66);
        float *angle = jyxx_get_angle();
        Angle_PID_control(angle[2]);

        if (track_data != 0) // 进入黑线
        {
            basic_speed = 30;
            car_state = 'D';
            motor_led_state = 1;
        }
    }
    else if (car_state == 'D')
    {
        Line_PID_control(track_data);
        if (track_data == 0)
        {
            car_state = 'A';
            basic_speed = 20;
            motor_led_state = 1;

            if (cnt++ == 2)
            {
                motor_mode = 0;
                return;
            }
        }
    }

    float left_encoder_speed = motor_get_speed(CFG_MOTOR_L_ID, 5);
    float right_encoder_speed = motor_get_speed(CFG_MOTOR_R_ID, 5);

    filtered_speed_left = SPEED_FILTER_ALPHA_LEFT * left_encoder_speed + (1.0f - SPEED_FILTER_ALPHA_LEFT) * filtered_speed_left;
    filtered_speed_right = SPEED_FILTER_ALPHA_RIGHT * right_encoder_speed + (1.0f - SPEED_FILTER_ALPHA_RIGHT) * filtered_speed_right;

    float output_speed_left = pid_calculate_incremental(&pid_speed_left, filtered_speed_left);
    float output_speed_right = pid_calculate_incremental(&pid_speed_right, filtered_speed_right);

    output_speed_left = pid_constrain(output_speed_left, pid_params_speed_left.out_min, pid_params_speed_left.out_max);
    output_speed_right = pid_constrain(output_speed_right, pid_params_speed_right.out_min, pid_params_speed_right.out_max);

    my_printf("left_encoder_speed=%.2f,output_speed_left=%.2f\r\n", left_encoder_speed, output_speed_left);
    my_printf("right_encoder_speed=%.2f,output_speed_right=%.2f\r\n", right_encoder_speed, output_speed_right);

    motor_set_speed(CFG_MOTOR_R_ID, output_speed_right);
    motor_set_speed(CFG_MOTOR_L_ID, output_speed_left);
}

/**
 * @brief       电机任务
 * @param       arg: 传入的参数
 * @retval      无
 */
static void app_motor_task(void *arg)
{
    TickType_t tick;
    tick = xTaskGetTickCount();
    // motor_pid_init();
    motor_set_speed(CFG_MOTOR_L_ID, 100);
    while (is_app_motor_task_create)
    {
        float right_encoder_speed = motor_get_speed(CFG_MOTOR_R_ID, 5);
        float left_encoder_speed = motor_get_speed(CFG_MOTOR_L_ID, 5);
        float ldistance = motor_get_now_distance(CFG_MOTOR_L_ID);
        float rdistance = motor_get_now_distance(CFG_MOTOR_R_ID);
        my_printf("lspeed:%.2f,distance=%.2f\r\n", left_encoder_speed, ldistance);
        my_printf("rspeed:%.2f,distance=%.2f\r\n", right_encoder_speed, rdistance);

        // jyxx_up_data();
        // track_read_sensors();
        // if (motor_mode == 0)
        // {
        //     motor_set_speed(CFG_MOTOR_R_ID, 0);
        //     motor_set_speed(CFG_MOTOR_L_ID, 0);
        // }
        // else if (motor_mode == 1)
        //     motor_mode1();
        // else if (motor_mode == 2)
        //     motor_mode2();
        // else if (motor_mode == 3)
        //     motor_mode3();
        // else if (motor_mode == 4)
        //     motor_mode4();

        vTaskDelayUntil(&tick, 5);
    }
}

/**
 * @brief       电机任务启动
 * @param       无
 * @retval      1:失败 0:成功
 */
uint8_t app_motor_task_start(void)
{
    if (is_app_motor_task_create != 0)
    {
        return 1;
    }

    if (xTaskCreate(app_motor_task, "app_motor_task", 1024, NULL, 3, NULL) != pdPASS)
    {
        return 1;
    }
    is_app_motor_task_create = 1;
    app_key_callback_register(app_motor_key_handle);
    return 0;
}

/**
 * @brief       电机任务停止
 * @param       无
 * @retval      无
 */
void app_motor_task_stop(void)
{
    is_app_motor_task_create = 0;
    app_key_callback_unregister(app_motor_key_handle);
}
